#include <slam_2d_ros/slam_2d_node.h>

Slam2D::Slam2D(){
    scanRawSub_ = nh_.subscribe("/scan_raw", 10, &Slam2D::ScanRawCallback, this);
    odomSub_ = nh_.subscribe("/odom", 10, &Slam2D::OdomRawCallback, this);
    imuSub_ = nh_.subscribe("/imu", 10, &Slam2D::ImuRawCallback, this);
    scanSlamPub_ = nh_.advertise<sensor_msgs::LaserScan>("/slam_2d_scan", 10);
    mapPub_ = nh_.advertise<nav_msgs::OccupancyGrid>("/slam_2d_map", 10);
    trajPub_ = nh_.advertise<nav_msgs::Path>("/slam_2d_traj", 10);
}

Slam2D::~Slam2D(){

}

// 雷达回调函数
void Slam2D::ScanRawCallback(const sensor_msgs::LaserScan::Ptr& msg){
    scanRawPtr_ = msg;
    mapping2D_.processScan2D(msg);

    sensor_msgs::LaserScan slam_scan = *msg;
    slam_scan.header.stamp = ros::Time::now();
    scanSlamPub_.publish(slam_scan);

    // 获取轨迹数据
    vector<Pose2D> traj_poses = mapping2D_.getTrajectory();
    if(!traj_poses.empty()){
        // 发布轨迹
        PublishTrajectory(trajPub_, traj_poses); 
    }

    // 发布TF  /map -> /bas_link
    Pose2D base_pose = mapping2D_.getCurrentPose();
    PublishBaseLinkTF(base_pose);

    // 发布地图
    // Vec2f origin;
    // cv::Mat global_map = mapping2D_.getMap(origin);
    // PublishGlobalMap(mapPub_, global_map, origin);

    // cv::imshow("global_map_test", global_map);
}

// 里程计回调函数
void Slam2D::OdomRawCallback(const nav_msgs::Odometry::Ptr& msg){
    odomRawPtr_ = msg;
}

// IMU回调函数
void Slam2D::ImuRawCallback(const sensor_msgs::Imu::Ptr& msg){
    imuRawPtr_ = msg;
}

// 发布地图
void Slam2D::PublishGlobalMap(ros::Publisher& map_pub, const cv::Mat& map_image, const Vec2f& origin){
    nav_msgs::OccupancyGrid ros_map;

    // 设置地图头信息
    ros_map.header.stamp = ros::Time::now();
    ros_map.header.frame_id = "map";

    // 设置地图信息
    ros_map.info.map_load_time = ros::Time::now();
    ros_map.info.resolution = 0.05;  // 地图分辨率（米/像素）
    ros_map.info.width = map_image.cols;
    ros_map.info.height = map_image.rows;

    // 设置地图原点
    ros_map.info.origin.position.x = -origin[0] * ros_map.info.resolution;  // 地图左下角物理坐标
    ros_map.info.origin.position.y = -origin[1] * ros_map.info.resolution;   
    ros_map.info.origin.orientation.w = 1.0;  // 默认方向

    // 将 OpenCV 的图像数据转为 ROS 地图数据
    ros_map.data.resize(map_image.cols * map_image.rows);
    for (int y = 0; y < map_image.rows; ++y) {
        for (int x = 0; x < map_image.cols; ++x) {
            uchar pixel = map_image.at<cv::Vec3b>(y, x)[0];  // 取蓝色通道
            if (pixel == 127) {
                ros_map.data[y * map_image.cols + x] = -1;  
            } else if (pixel > 127) {
                ros_map.data[y * map_image.cols + x] = 0;  
            } else {
                ros_map.data[y * map_image.cols + x] = 100;    
            }
        }
    }

    // 发布地图
    map_pub.publish(ros_map);
}


// 发布TF坐标
void Slam2D::PublishBaseLinkTF(const Pose2D& current_pose){
    static tf2_ros::TransformBroadcaster tf_broadcaster;

    geometry_msgs::TransformStamped tf_msg;

    tf_msg.header.stamp = ros::Time::now();
    tf_msg.header.frame_id = "map";
    tf_msg.child_frame_id = "base_link";

    // 将 Eigen::Isometry2f 转为 TransformStamped
    tf_msg.transform.translation.x = current_pose.translation().x();
    tf_msg.transform.translation.y = current_pose.translation().y();
    tf_msg.transform.translation.z = 0.0;  // 假设 2D 平面

    Eigen::Rotation2Dd rotation(current_pose.rotation());
    Eigen::Quaterniond quat(Eigen::AngleAxisd(rotation.angle(), Eigen::Vector3d::UnitZ()));
    tf_msg.transform.rotation.x = quat.x();
    tf_msg.transform.rotation.y = quat.y();
    tf_msg.transform.rotation.z = quat.z();
    tf_msg.transform.rotation.w = quat.w();

    // 广播 TF
    tf_broadcaster.sendTransform(tf_msg);
}

// 发布轨迹
void Slam2D::PublishTrajectory(ros::Publisher& path_pub, const std::vector<Pose2D>& traj_poses){
    nav_msgs::Path path_msg;
    path_msg.header.stamp = ros::Time::now();
    path_msg.header.frame_id = "map";

    for (const auto& pose : traj_poses) {
        geometry_msgs::PoseStamped pose_stamped;

        // 设置时间戳
        pose_stamped.header.stamp = ros::Time::now();
        pose_stamped.header.frame_id = "map";

        // 平移部分
        pose_stamped.pose.position.x = pose.translation().x();
        pose_stamped.pose.position.y = pose.translation().y();
        pose_stamped.pose.position.z = 0.0;  // 假设二维轨迹

        // 旋转部分 (由旋转矩阵生成四元数)
        Eigen::Rotation2Dd rotation(pose.rotation());
        Eigen::Quaterniond quat(Eigen::AngleAxisd(rotation.angle(), Eigen::Vector3d::UnitZ()));
        pose_stamped.pose.orientation.x = quat.x();
        pose_stamped.pose.orientation.y = quat.y();
        pose_stamped.pose.orientation.z = quat.z();
        pose_stamped.pose.orientation.w = quat.w();

        // 添加到路径
        path_msg.poses.push_back(pose_stamped);
    }

    // 发布路径
    path_pub.publish(path_msg);
}


int main(int argc, char** argv){
    FLAGS_alsologtostderr = true;           // 日志同时输出到stderr
    FLAGS_max_log_size = 1;                 // 单个日志文件大小上限（MB）, 如果设置为0将默认为1
	FLAGS_stop_logging_if_full_disk = true; // 当磁盘写满时，停止输出
    FLAGS_minloglevel = google::INFO;    // 只记录大于{}等级的日志
    FLAGS_log_dir = "/home/xxty/logs";
    google::InitGoogleLogging(argv[0]);

    ros::init(argc, argv, "slam_2d_ros");
    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>
    LOG(INFO) << "slam_2d_ros start...";

    Slam2D slam_2d;
    ros::spin();
    google::ShutdownGoogleLogging();
    return 0;
}